#include <stdio.h>
#include <cmath>
#include <sys/stat.h>
#include "cxcore.h"
#include "cv.h"
#include "highgui.h"
#include <iostream>

CvMat** m_gabor;
const int m_gabor_scales = 5;
const int m_gabor_orientations = 8;
const double m_gabor_fmax = M_PI/2.0;
const double m_gabor_sigma = 2*M_PI;
const int m_downscale = 8;

void save_mat_image(CvMat* mat, char* name, bool multichannel = false)
{
  CvMat* pmat;
  if(multichannel)
  {
    pmat = cvCreateMat(mat->rows, mat->cols, CV_64FC1);
    cvSplit(mat, pmat, NULL, NULL, NULL);
  }
  else
    pmat = mat;

	double maxVal,minVal;
	CvPoint minLoc, maxLoc;
	cvMinMaxLoc(pmat, &minVal, &maxVal,&minLoc, &maxLoc, 0 );
  IplImage tmp_header;
	IplImage* tt = cvGetImage(pmat, &tmp_header);
	IplImage* ipl = cvCreateImage(cvGetSize(tt) ,8,1);
	cvConvertScale(tt,ipl,255.0/(maxVal-minVal),-minVal*255.0/(maxVal-minVal));	
	cvSaveImage(name,ipl);
	cvReleaseImage(&ipl);

  if(mat != pmat)
    cvReleaseMat(&pmat);
}

void Mulfft3(const CvArr* srcAarr, const CvArr* srcBarr, CvArr* dstarr)
{
  CvMat *srcA = (CvMat*)srcAarr;
  CvMat *srcB = (CvMat*)srcBarr;
  CvMat *dst = (CvMat*)dstarr;

  for(int i = 0; i < srcA->rows; ++i)
  {
    for(int j = 0; j < srcA->cols; ++j)
    {
	    ((double*)(dst->data.ptr + dst->step*i))[j*2]	= ((double*)(srcA->data.ptr + srcA->step*i))[j*2]*((double*)(srcB->data.ptr + srcB->step*i))[j*2] -
	((double*)(srcA->data.ptr + srcA->step*i))[j*2+1]*((double*)(srcB->data.ptr + srcB->step*i))[j*2+1];
	    ((double*)(dst->data.ptr + dst->step*i))[j*2+1]	= ((double*)(srcA->data.ptr + srcA->step*i))[j*2]*((double*)(srcB->data.ptr + srcB->step*i))[j*2+1] +
((double*)(srcA->data.ptr + srcA->step*i))[j*2+1]*((double*)(srcB->data.ptr + srcB->step*i))[j*2];
    }
  }
}

void extract_features(const IplImage* img)
{		
  CvSize filter_size = cvGetSize(m_gabor[0]);
  CvSize img_size = cvGetSize(img);
 
	CvMat* imdft = cvCreateMat(filter_size.height, filter_size.width, CV_64FC2);
	cvZero(imdft);
	for(int i = 0; i < img_size.height; i++)
  {
		for(int j = 0; j < img_size.width; j++)
		{
			((double*)(imdft->data.ptr + imdft->step*i))[j*2] = (double)(CV_IMAGE_ELEM(img, uchar, i, j));
			((double*)(imdft->data.ptr + imdft->step*i))[j*2+1] = 0.0;
		}
  }
	cvDFT(imdft, imdft, CV_DXT_FORWARD);

	CvMat* gout = cvCreateMat(filter_size.height, filter_size.width, CV_64FC2);
 	for (int i=0;i<m_gabor_scales;i++)
	{
		for (int j=0;j<m_gabor_orientations;j++) 
		{
	    Mulfft3(imdft, m_gabor[i*m_gabor_orientations+j], gout);
 			cvDFT(gout, gout, CV_DXT_INVERSE);

			CvMat* gimg = cvCreateMat(img_size.height, img_size.width, CV_64F);
      for(int y = 0; y < img_size.height;  y++) 
      {
        for(int x = 0; x < img_size.width; x++)
        {
          ((double*)(gimg->data.ptr + gimg->step*y))[x] =
		        sqrt( ((double*)(gout->data.ptr + gout->step*(y+img_size.height/2)))[(x+img_size.width/2)*2]*
		         ((double*)(gout->data.ptr + gout->step*(y+img_size.height/2)))[(x+img_size.width/2)*2]+
		         ((double*)(gout->data.ptr + gout->step*(y+img_size.height/2)))[(x+img_size.width/2)*2+1]*
		         ((double*)(gout->data.ptr + gout->step*(y+img_size.height/2)))[(x+img_size.width/2)*2+1]);
        }
      }

      CvScalar mean = cvAvg(gimg);
      cvSubS(gimg, mean, gimg);
      cvNormalize(gimg, gimg);

			char coutfilename[512];
			sprintf(coutfilename,"gwt%d_%d.bmp",i,j);
			save_mat_image(gimg,coutfilename);
	    cvReleaseMat(&gimg);
		}
	}

	cvReleaseMat(&imdft);
  cvReleaseMat(&gout);		 	 
}

void create_gabor_filters(CvSize size)
{
  m_gabor = new CvMat*[m_gabor_scales*m_gabor_orientations];
	int dft_M = cvGetOptimalDFTSize(2*size.height - 1);
  int dft_N = cvGetOptimalDFTSize(2*size.width - 1);
	CvMat* filter = cvCreateMat(dft_M, dft_N, CV_64FC2);
  int offset_x = size.width/2, offset_y = size.height/2;
  double sigmasq = m_gabor_sigma*m_gabor_sigma;

  for(int v = 0; v < m_gabor_scales; v++)
  {
    for(int u = 0; u < m_gabor_orientations; u++)
    {
		  double fv = pow(sqrt(2.0),-v)*m_gabor_fmax;
		  double phiu = u*M_PI/m_gabor_orientations;
		  double fvsq = fv*fv;

			cvZero(filter);
		  for (int y = 0; y < size.height; y++) 
      {
			  for (int x = 0; x < size.width; x++)
        {
				  int i = x - offset_x; 
				  int j = y - offset_y;
				  double mag = (double)(i*i+j*j);
				  ((double*)(filter->data.ptr + filter->step*y))[x*2] = fvsq/sigmasq * exp(-0.5*fvsq*mag/sigmasq) * (cos(fv*(i*cos(phiu)+j*sin(phiu)))-exp(-1.0*sigmasq/2.0));
				  ((double*)(filter->data.ptr + filter->step*y))[x*2+1] = fvsq/sigmasq * exp(-0.5*fvsq*mag/sigmasq) * (sin(fv*(i*cos(phiu)+j*sin(phiu))));
   			}
	    }


			//char coutfilename[512];
			//sprintf(coutfilename,"spatial%d_%d.bmp",v,u);
			//save_mat_image(filter,coutfilename,true);

      m_gabor[v*m_gabor_orientations+u] = cvCreateMat( dft_M, dft_N, CV_64FC2);
 			cvDFT(filter, m_gabor[v*m_gabor_orientations+u], CV_DXT_FORWARD, size.height);

			//sprintf(coutfilename,"frequency%d_%d.bmp",v,u);
			//save_mat_image(m_gabor[v*m_gabor_orientations+u],coutfilename,true);
    }
  }

	cvReleaseMat(&filter);
	fprintf(stdout," OK!\n");
}

void release_gabor_filters()
{
  for(int i = 0; i < (m_gabor_scales*m_gabor_orientations); ++i)
    cvReleaseMat(&m_gabor[i]);
  delete [] m_gabor;
}

int main(int argc, char * argv[])
{
	// Load parameters
	if (argc<2)
	{
		fprintf(stdout,"\nSyntax error.\nUse: gabor <imagefile> <outputfile>\n");
	  return 0;
	}

	IplImage* img= cvLoadImage(argv[1],-1);
	if(img==0)
		fprintf(stdout,"Can't load image files!\n");	
  else
  {
    IplImage *hsv = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
    cvCvtColor(img, hsv, CV_RGB2HSV);
    cvSaveImage("hsv.jpg", hsv);
    cvReleaseImage(&hsv);

    create_gabor_filters(cvGetSize(img));
	  extract_features(img);
    release_gabor_filters();
  }

  return 0;
 }
